#pragma once

#include <rclcpp/rclcpp.hpp>
#include <navigation/deviation_controller.hpp>
#include <livox_ros_driver2/msg/custom_msg.hpp>
#include <pcl/common/common.h>
#include <navigation/avoid_obstacles/preprocess.hpp>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Matrix3x3.h>

/**
 * @brief AvoidObstacles class for processing obstacle avoidance using LiDAR data.
 * 
 * @author xhf
 */

class AvoidObstacles
{
public:
    AvoidObstacles(
        rclcpp::Node::SharedPtr node,
        std::shared_ptr<DeviationBaseController> deviation_controller) : node_(node), deviation_controller_(deviation_controller)
    {
        node_->declare_parameter("safe_x_range", std::vector<double>{0.7, 0.4});
        node_->declare_parameter("safe_y_range", std::vector<double>{0.7, 0.7});
        node_->declare_parameter("valid_z_range", std::vector<double>{0.1,1.3});
        node_->declare_parameter("angle_range", 30.0);
        node_->declare_parameter("obstacle_threshold", 30);
        node_->declare_parameter("obstacles_hz", 10.0);
        
        safe_x_far_ = node_->get_parameter("safe_x_range").as_double_array()[0];
        safe_x_near_ = node_->get_parameter("safe_x_range").as_double_array()[1];
        RCLCPP_INFO(node_->get_logger(), "Safe x range: [%.2f, %.2f]", safe_x_far_, safe_x_near_);
        safe_y_front_ = node_->get_parameter("safe_y_range").as_double_array()[0];
        safe_y_back_ = node_->get_parameter("safe_y_range").as_double_array()[1];
        z_min_ = node_->get_parameter("valid_z_range").as_double_array()[0];
        z_max_ = node_->get_parameter("valid_z_range").as_double_array()[1];
        angle_range_ = node_->get_parameter("angle_range").as_double();
        angle_range_cos_ = std::cos(angle_range_ * M_PI / 360.0);
        angle_range_sin_ = std::sin((180 - angle_range_) * M_PI / 360.0);
        obstacles_threshold_ = node_->get_parameter("obstacle_threshold").as_int();

        double hz = node_->get_parameter("obstacles_hz").as_double();
        
        sub_obstacles_ = node_->create_subscription<livox_ros_driver2::msg::CustomMsg>(
            "/livox/lidar", 1, std::bind(&AvoidObstacles::obstacles_callback, this, std::placeholders::_1));
        RCLCPP_INFO(node_->get_logger(), "AvoidObstacles initialized.");
        rate_ = std::make_shared<rclcpp::Rate>(hz);
    }

private:
    void obstacles_callback(const livox_ros_driver2::msg::CustomMsg::SharedPtr msg)
    {
        // Process the obstacles data and update the deviation controller
        // This is a placeholder for actual obstacle processing logic
        // RCLCPP_INFO(node_->get_logger(), "Received obstacles data with %zu points.", static_cast<size_t>(msg->point_num));
        
        double blind = 0.3;//左右剔除手臂的影响
        int count_x_front_far = 0, count_x_front_near = 0, count_x_back_far = 0, count_x_back_near = 0;
        int count_y_front = 0, count_y_back = 0;

        tf2::Matrix3x3 rotation_matrix;
        rotation_matrix.setEulerYPR(0, -30.0/180.0*M_PI, 0);

        for (const auto &point : msg->points)
        {
            tf2::Vector3 vec(point.x, point.y, point.z);
            vec = rotation_matrix * vec; // Apply rotation to the point
            double x = vec.x(), y = vec.y(), z = vec.z();


            double distance = std::sqrt(x * x + y * y);
            double cos_angle = x / distance;
            double sin_angle = y / distance;
            if (distance < safe_x_far_ && x > 0.1 && cos_angle > angle_range_cos_ && z > z_min_ && z < z_max_)
            {
                count_x_front_far++;
            }
            if (distance < safe_x_far_ && x < -0.1 && cos_angle < -angle_range_cos_ && z > z_min_ && z < z_max_)
            {
                count_x_back_far++;
            }

            if (distance < safe_x_near_ && x > 0.1 && cos_angle > angle_range_cos_ && z > z_min_ && z < z_max_)
            {
                count_x_front_near++;
            }
            if (distance < safe_x_near_ && x < -0.1 && cos_angle < -angle_range_cos_ && z > z_min_ && z < z_max_)
            {                
                count_x_back_near++;
            }


            if (distance > blind && distance < safe_y_front_ && y > 0 && sin_angle > angle_range_sin_ && z > z_min_ && z < z_max_)
            {
                count_y_front++;
            }
            if (distance > blind && distance < safe_y_back_ && y < 0 && sin_angle < -angle_range_sin_ && z > z_min_ && z < z_max_)
            {
                count_y_back++;
            }
            
        }

        // RCLCPP_INFO(node_->get_logger(), "Obstacles detected: x_front_far=%d, x_front_near=%d, x_back_far=%d, x_back_near=%d, y_front=%d, y_back=%d",
        //             count_x_front_far, count_x_front_near, count_x_back_far, count_x_back_near, count_y_front, count_y_back);


        bool obstacles_x_front_far = (count_x_front_far > obstacles_threshold_);
        bool obstacles_x_front_near = (count_x_front_near > obstacles_threshold_);
        bool obstacles_x_back_far = (count_x_back_far > obstacles_threshold_);
        bool obstacles_x_back_near = (count_x_back_near > obstacles_threshold_);
        bool obstacles_y_front = (count_y_front > obstacles_threshold_);
        bool obstacles_y_back = (count_y_back > obstacles_threshold_);

        if (obstacles_x_front_near)
        {
            deviation_controller_->x_lin_max.store(0.0);
        }else{
            if (obstacles_x_front_far)
            {
                deviation_controller_->x_lin_max.store(0.07);
            }else{
                deviation_controller_->x_lin_max.store(0.3);
            }
        }


        if (obstacles_x_back_near)
        {
            deviation_controller_->x_lin_min.store(0.0);
        }else{
            if (obstacles_x_back_far)
            {
                deviation_controller_->x_lin_min.store(-0.07);
            }else{
                deviation_controller_->x_lin_min.store(-0.3);
            }
        }

        if (obstacles_y_front)
        {
            deviation_controller_->y_lin_max.store(0.0);
        }else{
            deviation_controller_->y_lin_max.store(0.1);
        }
        if (obstacles_y_back)
        {
            deviation_controller_->y_lin_min.store(0.0);
        }else{
            deviation_controller_->y_lin_min.store(-0.1);
        }

        // 用于调试
        // RCLCPP_INFO(node_->get_logger(),"x_lin_min = %f, x_lin_max = %f, y_lin_min = %f, y_lin_max = %f",
        //     deviation_controller_->x_lin_min.load(),
        //     deviation_controller_->x_lin_max.load(),
        //     deviation_controller_->y_lin_min.load(),
        //     deviation_controller_->y_lin_max.load());

        rate_->sleep();
    }

    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<DeviationBaseController> deviation_controller_;
    rclcpp::Subscription<livox_ros_driver2::msg::CustomMsg>::SharedPtr sub_obstacles_;
    std::vector<double> safe_x_range_; // Safe x range for obstacle avoidance
    std::shared_ptr<rclcpp::Rate> rate_;

    double angle_range_, angle_range_cos_, angle_range_sin_;
    double safe_x_far_, safe_x_near_, safe_y_front_, safe_y_back_, z_min_, z_max_;
    int obstacles_threshold_;
};

